/*
 * BeacManageM.nc
 * David Moore <dcm@mit.edu>
 *
 * Module for handling distance measurements to neighboring beacons.  Keeps
 * track of recent measurements, and purges outdated ones.  Also allows
 * measurements to be relayed to other nodes and handles received relays.
 *
 * Provides the ability to dump these measurements to the serial port.
 * Although no specific query functions are implemented, one can write them
 * as necessary for the specific application.
 *
 * TODO: accurate timestamping capability
 */

/*
      This software may be used and distributed according to the terms
      of the GNU General Public License (GPL), incorporated herein by reference.
      Drivers based on this skeleton fall under the GPL and must retain
      the authorship (implicit copyright) notice.

      This program is distributed in the hope that it will be useful, but
      WITHOUT ANY WARRANTY; without even the implied warranty of
      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
      General Public License for more details.
*/

module BeacManageM {
    provides {
        interface StdControl;
        interface BeacManage;
    }
    uses {
        interface Timer as AgeTimer;
    }
}

#define MAX_NEIGHBORS   20
#define SELF            MAX_NEIGHBORS
#define MAX_DISTANCE	5

#define EMPTY           0xff
#define RELAY_DATA      0x1
#define OLD_MEAS        0x2
#define OLD_RELAY       0x4

#define MEAS_EXPIRE_SECONDS 10

implementation {
#define DISTANCE_MULT 30

    /* Information about a neighboring node */
    struct NodeInfo {
        uint8_t id[4];      /* Its 4-byte ID */
	uint8_t space[8];   /* Its 8-byte Space ID */
        uint8_t status;     /* status of our distance cache */
        uint8_t pos; 	    /* Position in distance measurments */
        uint8_t next;       /* pointer to next node for hash table collisions */
    };

    /* dist[][] holds the up to MAX_DISTANCE measurements to each beacon */
    uint16_t dist[MAX_NEIGHBORS+1][MAX_DISTANCE+1];
    /* neighbors[] is a record of each node that has distance information. */
    struct NodeInfo neighbors[MAX_NEIGHBORS+1];
    /* a simple hash table so we don't have to loop through a long list
     * comparing IDs */
    uint8_t id_hash[127];
 
    uint8_t current_beacon;

    command result_t StdControl.init() {
        uint8_t i;

        /* Initialize the list of nodes and the hash table */
        for (i=0; i<=MAX_NEIGHBORS; i++)
            neighbors[i].status = EMPTY;
        for (i=0; i<0x7f; i++)
            id_hash[i] = EMPTY;

        return SUCCESS;
    }

    command result_t StdControl.start() {
        /* Start a timer for purging stale distances. */
        call AgeTimer.start(TIMER_REPEAT, 1024*MEAS_EXPIRE_SECONDS);
        return SUCCESS;
    }

    command result_t StdControl.stop() {
        call AgeTimer.stop();
        return SUCCESS;
    }

    /* Copies an ID */
    void copy_id(uint8_t * dst, uint8_t * src)
    {
        uint8_t i;

        for (i=0; i<8; i++)
            dst[i] = src[i];
    }

    /* Deletes a node from our cache.  Each node has a 4-byte ID (from hardware)
     * and an 8-bit number which is its index into neighbors[].  We use the 8-bit
     * number for most operations since it's short and easy to deal with.
     * The node argument to node_delete() is this 8-bit index. */
    void node_delete(uint8_t node)
    {
        uint8_t next = neighbors[node].next;
        uint8_t id = neighbors[node].id[3];

        /* Empty the entry in neighbors[] */
        neighbors[node].status = EMPTY;

        /* Remove the node from the hash table */
        if (id_hash[id&0x7f] == node) {
            id_hash[id&0x7f] = next;
            return;
        }

        id = id_hash[id&0x7f];
        while (neighbors[id].next != node)
            id = neighbors[id].next;

        neighbors[id].next = next;
    }
    
    /* Finds a node in the list of known nodes given its 4-byte ID. */
    uint8_t node_find(uint8_t * id) {
        uint8_t node = id_hash[id[3]&0x7f];
        uint8_t i;

        while (node != EMPTY) {
            uint8_t match = 1;
            for (i=0; i<4; i++)
                if (id[i] != neighbors[node].id[i])
                    match = 0;
            
            if (match)
                return node;
            
            node = neighbors[node].next;
        }
        return EMPTY;
    }

    /* Helper function to node_add().  Stores a new node in a specified
     * slot of the array. */
    void node_add_slot(uint8_t * id, uint8_t slot)
    {
        uint8_t * ptr;

        neighbors[slot].status = 0;
        neighbors[slot].next = EMPTY;
        neighbors[slot].pos = 0;
        dist[slot][0] = 0;
        copy_id(neighbors[slot].id, id);

        ptr = &(id_hash[id[3]&0x7f]);
        while (*ptr != EMPTY) {
            ptr = &(neighbors[*ptr].next);
        }

        *ptr = slot;
    }

    /* Adds a new node to the array of known neighbors.  Returns its index
     * in the array. */
    uint8_t node_add(uint8_t * id) {
        uint8_t i, node=0;

        for (i=0; i<MAX_NEIGHBORS; i++) {
            if (neighbors[i].status == EMPTY) {
                node = i;
                break;
            }
        }
        if (i == MAX_NEIGHBORS)
            return EMPTY;

        node_add_slot(id, node);
        return node;
    }

    /* Purges old entries from the cache */
    event result_t AgeTimer.fired() {
        uint8_t i;

        for (i=0; i<MAX_NEIGHBORS; i++) {
            if (neighbors[i].status == EMPTY)
                continue;

            /* If the measurement hasn't been updated since our last timer
             * firing, delete the node. */
            if (neighbors[i].status & OLD_MEAS) {
                node_delete(i);
                continue;
            }

            /* If the relay data hasn't been updated since our last timer
             * firing, mark the data as not present. */
            if (neighbors[i].status & OLD_RELAY)
                neighbors[i].status &= ~RELAY_DATA;

            /* Set the aging flags. */
            neighbors[i].status |= OLD_MEAS;
            neighbors[i].status |= OLD_RELAY;
        }
        return SUCCESS;
    }

    /* Specifies our local ID.  This is necessary so we recognize distances to
     * ourselves measured by other nodes. */
    command result_t BeacManage.SetId(uint8_t * id)
    {
        node_add_slot(id, SELF);
        neighbors[SELF].status |= RELAY_DATA;
        return SUCCESS;
    }

    /* Stores a new measurement between the local node and a neighbor
     * specified by its ID.  The distance should be in microseconds. */
    command result_t BeacManage.NewMeasurement(uint8_t * id, uint16_t d)
    {
        uint8_t node;

        /* Find the ID in our list, or create a new entry if we haven't
         * seen this node before. */
        node = node_find(id);
        if (node == EMPTY) {
            node = node_add(id);
            if (node == EMPTY)
                return FAIL;
        }

        /* Clear the aging flag */
        neighbors[node].status &= ~OLD_MEAS;
        /* Store the distance */
	if (neighbors[node].pos < MAX_DISTANCE)
		dist[node][neighbors[node].pos] = d;
	else {
		neighbors[node].pos = 0;	
		dist[node][0] = d;
	}
	neighbors[node].pos++;
        return SUCCESS;
    }

    /* Fills a packet of data with distances measured locally suitable for
     * being relayed to neighboring nodes. */
    command uint16_t BeacManage.GetMeasurements(uint8_t * id, uint8_t pos)
    {
        uint8_t node;
	uint8_t print_pos;
	

        node = node_find(id);
        if (node == EMPTY) {
            node = node_add(id);
            if (node == EMPTY)
                return FAIL;
        }	

	if ( pos > MAX_DISTANCE) pos = MAX_DISTANCE;

	print_pos = neighbors[node].pos;
	if (print_pos == 0) {
		print_pos = MAX_DISTANCE; 
	} else {
		print_pos -= 1;
	}
	if (pos <= print_pos) print_pos -= pos;
	if (pos > print_pos) print_pos += (pos - print_pos);
	if (print_pos > MAX_DISTANCE) print_pos -= MAX_DISTANCE;

	return dist[node][print_pos];
    }

    command uint8_t BeacManage.ResetBeaconId()
    {
	//uint8_t i;
	current_beacon = 0;

	/*for (i=0; i<=MAX_NEIGHBORS; i++) {
		if (neighbors[i].status != EMPTY) {
		   copy_id(id, neighbors[i].id);
		   current_beacon = i;
                   return SUCCESS;
		}
	}*/	
	return FAIL;
    }

    command uint8_t BeacManage.GetNextBeaconId(uint8_t *id)
    {
        uint8_t i;
        for (i=current_beacon; i<=MAX_NEIGHBORS-1; i++) {
                if (neighbors[i].status != EMPTY) {
                   copy_id(id, neighbors[i].id);
		   current_beacon = i+1;
                   return SUCCESS;
                }
        }
	return FAIL;
    }

    /* Print the contents of the distance table to the serial port 
     * for all distances measured by a specific node. */
    void debug_print(int node)
    {
        uint8_t c, i;

        //if (node == SELF) {
        //    UARTOutput(DBG_ERROR, "  from 0x%02x (self): ",
        //            neighbors[node].id[3]);
        //}
        //else
        //    UARTOutput(DBG_ERROR, "  from 0x%02x       : ",
        //            neighbors[node].id[3]);

        c=0;
        for (i=0; i<=MAX_NEIGHBORS; i++) {
            if (dist[node][i] != 0 && neighbors[i].status != EMPTY) {
                if (c != 0 && ((c&0x3) == 0))
        //            UARTOutput(DBG_ERROR, "\r\n          ");
        //        UARTOutput(DBG_ERROR, "0x%02x d=%3d; ", neighbors[i].id[3],
        //                dist[node][i] / DISTANCE_MULT);
                c++;
            }
        }
        //UARTOutput(DBG_ERROR, "\r\n");
    }

    /* Print the entire contents of the distance table to the serial port
     * if id is NULL.  Otherwise, print only the distances measured by that
     * node. */
    command result_t BeacManage.DebugPrint(uint8_t * id)
    {
        uint8_t node;
        if (id) {
            node = node_find(id);
            if (node == EMPTY)
                return FAIL;
            if (neighbors[node].status & RELAY_DATA)
                debug_print(node);
            return SUCCESS;
        }
        
        for (node=0; node<=MAX_NEIGHBORS; node++) {
            if (neighbors[node].status != EMPTY &&
                    neighbors[node].status & RELAY_DATA)
                debug_print(node);
        }
        return SUCCESS;
    }
}
